#!/usr/bin/env python
# -*- coding: utf-8 -*-
import local_pyhop as ph
import numpy as np
import action_point as ap
#复合任务：用于定义分解任务的方法。原子任务：每个原子任务的执行都会导致state的变化。


#用于领域知识中原子任务和复合任务的调用
def infer_move_point(area):
    '''为无人机推理移动目标点'''
    global pos_move
    X = abs(area[0][1] - area[0][0])
    Y = abs(area[1][1] - area[1][0])
    if X*Y <= 25:
        if area[0][0] < area[0][1]:
            pos_move = [area[0][0] + X/2, area[1][0] + Y/2, 3]
        elif area[0][0] > area[0][1]:
            pos_move = [area[0][1], area[1][0], 3]
    elif X*Y > 25 and X*Y <= 40:
        if X >= Y :
            pos_move = [[area[0][0] + X/4, area[1][0] + Y/2, 3],]
            pos_move.append([area[0][0] + X*3/4, area[1][0] + Y/2, 3])
        else :
            pos_move = [[area[0][0] + X/2, area[1][0] + Y/4, 3],]
            pos_move.append([area[0][0] + X/2, area[1][0] + Y*3/4, 3])
    elif X * Y >= 40 and X * Y <=100:
        pos_move = [[area[0][0] + X/4, area[1][0] + Y/4, 3],]
        pos_move.append([area[0][0] + X*3/4, area[1][0] + Y/4, 3])
        pos_move.append([area[0][0] + X*3/4, area[1][0] + Y* 3/4, 3])
        pos_move.append([area[0][0] + X/4, area[1][0] + Y*3/4, 3])
    return pos_move


def infer_move_close(state):
    pos_move = np.zeros((state.target.num_arucos, 3))
    if state.target.num_arucos >= 0:
        for i in range(state.target.num_arucos):
            ap.multi_aruco_pos_enu[0] = ap.mav_pos_[0] - state.target.aruco_infos[i].position[1]
            ap.multi_aruco_pos_enu[1] = ap.mav_pos_[1] - state.target.aruco_infos[i].position[0]
            ap.multi_aruco_pos_enu[2] = 0.5
            pos_move[i] = ap.multi_aruco_pos_enu
    return pos_move



def infer_grade_area(area):
    '''确定无人机执行任务区域等级'''
    global grade
    X = abs(area[0][1] - area[0][0])
    Y = abs(area[1][1] - area[1][0])
    if X*Y <= 25:
        grade = 1
    elif X*Y > 25 and X*Y <= 40:
        grade = 2
    elif X * Y > 40:
        grade = 4
    return grade


# ======== 原子任务的定义 ========
def move(state, pos_move):
    """“移动”原子任务"""
    state.pos = pos_move
    return state


def arm_takeoff(state):
    """“解锁起飞”原子任务"""
    state.mode = "OFFBOARD"
    return state


def land_disarm(state):
    """“降落上锁”原子任务"""
    state.mode = "MANUAL"
    return state


def scan_high(state,target):
    """“高空扫描”原子任务"""
    return state


def scan(state,target):
    """“扫描”原子任务"""
    return state


ph.declare_operators(move, arm_takeoff,land_disarm, scan_high,scan)



# =================== 复合任务的定义 ========================
# ------------------- 机动：maneuver ------------------------
def maneuver(state,pos_move):
    """无人机机动任务的分解"""
    if state.mode == "OFFBOARD" :
        return [('move', pos_move)]
    elif state.mode == "MANUAL":
        return [('arm_takeoff',),('move', pos_move)]

ph.declare_methods('maneuver_task', maneuver)



# ------------------- 返回降落：return_land ------------------------
def return_land(state,pos_land):
    """返回降落任务的分解"""
    if abs(state.pos[0] - pos_land[0]) < 0.15 and \
            abs(state.pos[1] - pos_land[1]) < 0.15 and \
            abs(state.pos[2] - pos_land[2]) < 0.15:
        return [('land_disarm',)]
    else:
        return [('maneuver_task', pos_land), ('land_disarm',)]

ph.declare_methods('return_land_task', return_land)



# ------------------- 区域侦察：scout_area ------------------------
def scout_area(state,area,target):
    """区域侦察任务的分解"""
    grade = infer_grade_area(area)
    pos_move = infer_move_point(area)
    pos_land = state.pos_land
    mark = 0
    return [('scout_large_task', pos_move,grade,mark,target), ('return_land_task', pos_land)]

ph.declare_methods('scout_area_task', scout_area)



# ------------------- 大区域侦察：scout_large ------------------------
def scout_large(state,pos_move,grade,mark,target,):
    """大区域侦察任务的分解"""
    mark += 1
    if mark < grade :
        return [('scout_unit_task', pos_move,mark,target), ('scout_large_task', pos_move,grade,mark,target)]
    else :
        return [('scout_unit_task', pos_move,mark,target)]

ph.declare_methods('scout_large_task', scout_large)



# ------------------- 单位区域侦察：area_unit ------------------------
def scout_unit(state,pos_move,mark,target):
    """区域侦察任务的分解"""

    return [('maneuver_task', pos_move[mark-1]), ('scan_high', target), ('scan_close_task', target,mark)]

ph.declare_methods('scout_unit_task', scout_unit)



# ------------------- 靠近扫描：scan_close ------------------------
def scan_close(state,target,mark):
    """“靠近扫描复合任务的分解”"""
    if state.target_num[mark-1] == None:
        return False
    else:
        pos_move = infer_move_close(state)
        if state.target_num[mark - 1] == 0:
            plan = []
            return plan

        plan = [('maneuver_task', pos_move[0]), ('scan', target)]
        for i in range(state.target_num[mark-1]-1):
            plan = plan + [('maneuver_task', pos_move[i+1]),('scan',target)]
        return plan

ph.declare_methods('scan_close_task', scan_close)
